/*
 * Copyright (c) 2020-2021, SERI Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-06-23     Lyons        first version
 */

#include "__datadef.h"

/**
  * @brief Define Global Parameters
  */

static pthread_mutex_t _s_kb_mutex;

#define _KB_LOCK()              pthread_mutex_lock(&_s_kb_mutex)
#define _KB_UNLOCK()            pthread_mutex_unlock(&_s_kb_mutex)

static uint32_t _s_trig = 0;
static uint32_t _s_quit = 0;

static uint32_t _s_irq = 0;

#include "cli.c"
#include "shell.c"

static struct xcli _s_cli = {
    .arg_filename = "./riscv.bin",
};

/**
  * @brief Define Memory and Peripheral
  */

Memory_t rom;
Memory_t ram;

#ifdef ENABLE_TIMER
Memory_t timer;
#endif
#ifdef ENABLE_UART
Memory_t uart1;
#endif
#ifdef ENABLE_GPIO
Memory_t gpio;
#endif

Regfile_t iregfile; // i-regfile
Csrfile_t csrfile; // csr regfile

static MemoryMap_t _s_memory_map [] =
{
    { .name = "rom",    .address = 0x00000000, .size = MEMORY_SIZE, .memory = &rom,   },
    { .name = "ram",    .address = 0x10000000, .size = MEMORY_SIZE, .memory = &ram,   },
#ifdef ENABLE_TIMER
    { .name = "timer",  .address = 0x20000000, .size = 4*1024,      .memory = &timer, },
#endif
#ifdef ENABLE_UART
    { .name = "uart1",  .address = 0x30000000, .size = 4*1024,      .memory = &uart1, },
#endif
#ifdef ENABLE_GPIO
    { .name = "gpio",   .address = 0x40000000, .size = 4*1024,      .memory = &gpio,  },
#endif
};

// define 16-irq here
// timer1 as systick

#define INT_IRQ_TIMER1          SHIFT_L(1, 0 ) // high priority 
#define INT_IRQ_RSV1            SHIFT_L(1, 1 ) 
#define INT_IRQ_RSV2            SHIFT_L(1, 2 ) 
#define INT_IRQ_RSV3            SHIFT_L(1, 3 ) 
#define INT_IRQ_RSV4            SHIFT_L(1, 4 ) 
#define INT_IRQ_RSV5            SHIFT_L(1, 5 ) 
#define INT_IRQ_RSV6            SHIFT_L(1, 6 ) 
#define INT_IRQ_RSV7            SHIFT_L(1, 7 ) 
#define INT_IRQ_UART1           SHIFT_L(1, 8 )
#define INT_IRQ_UART2           SHIFT_L(1, 9 )
#define INT_IRQ_UART3           SHIFT_L(1, 10)
#define INT_IRQ_RSV11           SHIFT_L(1, 11) 
#define INT_IRQ_RSV12           SHIFT_L(1, 12) 
#define INT_IRQ_RSV13           SHIFT_L(1, 13) 
#define INT_IRQ_RSV14           SHIFT_L(1, 14) 
#define INT_IRQ_RSV15           SHIFT_L(1, 15) // low priority 

#define CSR_MTVEC               0x305
#define CSR_MEPC                0x341
#define CSR_MCAUSE              0x342
#define CSR_MIE                 0x304
#define CSR_MIP                 0x344
#define CSR_MTVAL               0x343
#define CSR_MSCRATCH            0x340
#define CSR_MSCRATCHCSWL        0x349
#define CSR_MSTATUS             0x300
#define CSR_CYCLEH              0xc80
#define CSR_CYCLE               0xc00

/**
  * @brief  Find Memory/Peripheral by Name
  * @note   
  * @param  *name, name of memory/peripheral
  * @retval MemoryMap_t*
  */
static MemoryMap_t* _find_by_name(const char *name)
{
    MemoryMap_t *pm = NULL;

    for (uint32_t i=0; i<GET_ARRAY_NUM(_s_memory_map); i++)
    {
        if ( 0 == strcmp(name, (const char*)_s_memory_map[i].name) )
        {
            pm = &_s_memory_map[i];
            break;
        }
    }

    return pm;
}

/**
  * @brief  Find Memory/Peripheral by Address
  * @note   
  * @param  addr, address of memory/peripheral
  * @retval MemoryMap_t*
  */
static MemoryMap_t* _find_by_addr(uint32_t addr)
{
    MemoryMap_t *pm = NULL;

    for (uint32_t i=0; i<GET_ARRAY_NUM(_s_memory_map); i++)
    {
        if ( DATA_MASK(addr, 4, 28) == DATA_MASK(_s_memory_map[i].address, 4, 28) )
        {
            pm = &_s_memory_map[i];
            break;
        }
    }

    return pm;
}

/**
  * @brief  Initialize the Memory and Regfile of CPU
  * @note   Peripheral are Considered as Memory
  * @param  void
  * @retval int
  */
int memory_init(void)
{
    CLEAR_STRUCT(rom, 0);
    CLEAR_STRUCT(ram, 0);
#ifdef ENABLE_TIMER
    CLEAR_STRUCT(timer, 0);
#endif
#ifdef ENABLE_UART
    CLEAR_STRUCT(uart1, 0);
#endif
#ifdef ENABLE_GPIO
    CLEAR_STRUCT(gpio,  0);
#endif

    for (uint32_t i=0; i<GET_ARRAY_NUM(_s_memory_map); i++) {
        _s_memory_map[i].memory->size = _s_memory_map[i].size / 4;
    }

    CLEAR_STRUCT(iregfile, 0);
    iregfile.size = GET_ARRAY_NUM(iregfile.buf);

    CLEAR_STRUCT(csrfile, 0);
    csrfile.size = GET_ARRAY_NUM(csrfile.buf);

    return 0;
}

/**
  * @brief  Load Program Image
  * @note   Fill 0 with Unused Memory Space
  * @param  *path, program image file path
  *         *mem, memory to store instruction
  * @retval int
  */
int load_image(const char *path, Memory_t *mem)
{
    FILE *fd;

    fd = fopen(path, "rb");

    if (!fd)
    {
        DEBUG_D("missing image file!\n");
        return -1;
    }

    uint32_t _bin_size;
    uint32_t _inst_size;

    uint8_t _bin_buf[MEMORY_SIZE];
    CLEAR_ARRAY(_bin_buf, 0);

    _bin_size = fread(_bin_buf, sizeof(uint8_t), sizeof(_bin_buf), fd);
    _inst_size = (_bin_size + 3) / 4;

    for (uint32_t i=0; i<_inst_size; i++)
    {
        mem->buf[i] = (_bin_buf[i*4 + 0] << 0 )
                    + (_bin_buf[i*4 + 1] << 8 )
                    + (_bin_buf[i*4 + 2] << 16)
                    + (_bin_buf[i*4 + 3] << 24);
    }

    fclose(fd);
    return 0;
}

/**
  * @brief  Regfile Operation
  * @note   in i-regfile, x0 is lways '0'
  *         in csr regfile, 0x000/0x001 is always '0'
  * @param  *inst_info, instruction information
  * @retval void
  */
static void _regfile_read(InstInfo_t *inst_info)
{
    inst_info->rs1_data = (0 == inst_info->rs1) ? 0 : iregfile.buf[inst_info->rs1];
    inst_info->rs2_data = (0 == inst_info->rs2) ? 0 : iregfile.buf[inst_info->rs2];
}

static void _regfile_write(InstInfo_t *inst_info)
{
    iregfile.buf[inst_info->rd] = (0 == inst_info->rd) ? 0 : inst_info->rd_data;
}

static void _csrfile_read(InstInfo_t *inst_info)
{
    inst_info->csr_data = 0;
    if ( 0x000 != inst_info->csr && 0x001 != inst_info->csr ) {
        inst_info->csr_data = csrfile.buf[inst_info->csr];
    }
}

static void _csrfile_write(InstInfo_t *inst_info)
{
    if ( CSR_MSTATUS == inst_info->csr && 0x00001880 == csrfile.buf[CSR_MSTATUS] ) {
        return;
    }

    if ( 0x000 != inst_info->csr && 0x001 != inst_info->csr )
    {
        csrfile.buf[inst_info->csr] = inst_info->csr_data;
    } else {
        csrfile.buf[inst_info->csr] = 0;
    }
}

/**
  * @brief  Dump Register
  * @note   
  * @param  *info, info string
  *         *inst_info, instruction information
  * @retval int
  */
static int _dumpreg(const char *info, InstInfo_t *inst_info)
{
    if (info)
    {
        DEBUG_D("\n%s\n", info);
    } else {
        DEBUG_D("\n");
    }

    DEBUG_D("pc [%08x] memory [%08x]\n", inst_info->current_pc, inst_info->mem_addr);

    DEBUG_D("\ni-regfile data:\n");

    DEBUG_D("zero: %08x ra: %08x sp:  %08x gp:  %08x\n", iregfile.buf[0],  iregfile.buf[1],  iregfile.buf[2],  iregfile.buf[3] );
    DEBUG_D("tp:   %08x t0: %08x t1:  %08x t2:  %08x\n", iregfile.buf[4],  iregfile.buf[5],  iregfile.buf[6],  iregfile.buf[7] );
    DEBUG_D("s0:   %08x s1: %08x a0:  %08x a1:  %08x\n", iregfile.buf[8],  iregfile.buf[9],  iregfile.buf[10], iregfile.buf[11]);
    DEBUG_D("a2:   %08x s3: %08x a4:  %08x a5:  %08x\n", iregfile.buf[12], iregfile.buf[13], iregfile.buf[14], iregfile.buf[15]);
    DEBUG_D("a6:   %08x a7: %08x s2:  %08x s3:  %08x\n", iregfile.buf[16], iregfile.buf[17], iregfile.buf[18], iregfile.buf[19]);
    DEBUG_D("s4:   %08x s5: %08x s6:  %08x s7:  %08x\n", iregfile.buf[20], iregfile.buf[21], iregfile.buf[22], iregfile.buf[23]);
    DEBUG_D("s8:   %08x s9: %08x s10: %08x s11: %08x\n", iregfile.buf[24], iregfile.buf[25], iregfile.buf[26], iregfile.buf[27]);
    DEBUG_D("t3:   %08x t4: %08x t5:  %08x t6:  %08x\n", iregfile.buf[28], iregfile.buf[29], iregfile.buf[30], iregfile.buf[31]);

    _s_quit = 1;

    return 0;
}

/**
  * @brief  Dump Memory
  * @note   
  * @param  *info, info string
  *         addr, memory address
  *         len, memory length
  * @retval int
  */
static int _dumpmem(const char *info, uint32_t addr, int len)
{
#ifndef READ_BYTE
#define READ_BYTE(addr) ( SHIFT_R(pm->memory->buf[(addr)/4], (((addr)%4)*8)) & 0xff )
#endif

    if (info)
    {
        DEBUG_D("\n%s\n", info);
    } else {
        DEBUG_D("\n");
    }

    MemoryMap_t *pm = _find_by_addr(addr);

    if ( !pm ) {
        return -1;
    }

    uint32_t base = DATA_MASK(addr, 4, 28);
    uint32_t offset = DATA_MASK(addr, 28, 0);

    DEBUG_D("        "); // NOTE: 8-block
    for (int i=0; i<16; i++) {
        DEBUG_D("%c%02x", (((i%16) == 8) ? '-' : ' '), i);
    }
    DEBUG_D("\n");

    uint32_t start = (offset / 16) * 16;
    uint32_t end = ((offset + len + 15) / 16) * 16;

    uint8_t line[16] = {0};
    uint8_t mask[16] = {0};

    for (int index=start; index<end; index++)
    {
        if ( (index % 16) == 0 ) {
            DEBUG_D("%08x", index);
        }

        if ( (index >= offset) && (index < (offset + len)) )
        {
            line[index%16] = READ_BYTE(index);
            mask[index%16] = 1;
            DEBUG_D("%c%02x", (((index%16) == 8) ? '-' : ' '), line[index%16]);
        } else {
            line[index%16] = 0;
            mask[index%16] = 0;
            DEBUG_D("   ");
        }

        if ( (index % 16) == 15 )
        {
            DEBUG_D(" ");
            for (int i=0; i<16; i++)
            {
                if ( mask[i] )
                {
                    DEBUG_D("%c", (isprint(line[i]) ? line[i] : '.'));
                } else {
                    DEBUG_D(" ");
                }
            }
            DEBUG_D("\n");
        }
    }

    return 0;
}

/**
  * @brief  Instruction Decoding
  * @note   
  * @param  inst, current instruction data
  *         *inst_info, instruction information
  * @retval int
  */

#include "exec.c"

static int _inst_decode(uint32_t inst, InstInfo_t *inst_info)
{
    uint8_t  opcode;
    uint8_t  func3;
    uint8_t  func7;
    uint8_t  rs1;
    uint8_t  rs2;
    uint8_t  rd;

    uint32_t imm_i; // imm12
    uint32_t imm_s; // imm12
    uint32_t imm_b; // imm12
    uint32_t imm_u; // imm20
    uint32_t imm_j; // imm20

    opcode =  DATA_MASK( inst_info->inst, 7,  0  );
    func3  =  DATA_MASK( inst_info->inst, 3,  12 );
    func7  =  DATA_MASK( inst_info->inst, 7,  25 );
    rs1    =  DATA_MASK( inst_info->inst, 5,  15 );
    rs2    =  DATA_MASK( inst_info->inst, 5,  20 );
    rd     =  DATA_MASK( inst_info->inst, 5,  7  );

    imm_i  =  DATA_MASK( inst_info->inst, 12, 20 );
    imm_s  = (DATA_MASK( inst_info->inst, 7,  25 ) << 5 )
           | (DATA_MASK( inst_info->inst, 5,  7  ) << 0 );
    imm_b  = (DATA_MASK( inst_info->inst, 1,  31 ) << 11)
           | (DATA_MASK( inst_info->inst, 6,  25 ) << 4 )
           | (DATA_MASK( inst_info->inst, 4,  8  ) << 0 )
           | (DATA_MASK( inst_info->inst, 1,  7  ) << 10);
    imm_u  =  DATA_MASK( inst_info->inst, 20, 12 );
    imm_j  = (DATA_MASK( inst_info->inst, 1,  31 ) << 19)
           | (DATA_MASK( inst_info->inst, 10, 21 ) << 0 )
           | (DATA_MASK( inst_info->inst, 1,  20 ) << 10)
           | (DATA_MASK( inst_info->inst, 8,  12 ) << 11);

    switch (opcode)
    {
        case 0x37 : {       // LUI
            inst_info->rd    = rd;
            inst_info->imm   = imm_u; // imm20
            inst_info->exec  = (void*)exec_lui;
            break;
        }
        case 0x17 : {       // AUIPC
            inst_info->rd    = rd;
            inst_info->imm   = imm_u; // imm20
            inst_info->exec  = (void*)exec_auipc;
            break;
        }
        case 0x6f : {       // JAL
            inst_info->rd    = rd;
            inst_info->imm   = imm_j; // imm20
            inst_info->exec  = (void*)exec_jal;
            break;
        }
        case 0x67 : {       // JALR
            inst_info->rs1   = rs1;
            inst_info->rd    = rd;
            inst_info->imm   = imm_i; // imm12
            inst_info->exec  = (void*)exec_jalr;
            break;
        }
        case 0x63 : {       // B..
            inst_info->rs1   = rs1;
            inst_info->rs2   = rs2;
            inst_info->imm   = imm_b; // imm12
            switch (func3)
            {
                case 0x0: inst_info->exec = (void*)exec_beq;  break;
                case 0x1: inst_info->exec = (void*)exec_bne;  break;
                case 0x4: inst_info->exec = (void*)exec_blt;  break;
                case 0x5: inst_info->exec = (void*)exec_bge;  break;
                case 0x6: inst_info->exec = (void*)exec_bltu; break;
                case 0x7: inst_info->exec = (void*)exec_bgeu; break;
            }
            break;
        }
        case 0x03 : {       // Load
            inst_info->rs1   = rs1;
            inst_info->rd    = rd;
            inst_info->imm   = imm_i; // imm12
            switch (func3)
            {
                case 0x0: inst_info->exec = (void*)exec_lb;  break;
                case 0x1: inst_info->exec = (void*)exec_lh;  break;
                case 0x2: inst_info->exec = (void*)exec_lw;  break;
                case 0x4: inst_info->exec = (void*)exec_lbu; break;
                case 0x5: inst_info->exec = (void*)exec_lhu; break;
            }
            break;
        }
        case 0x23 : {       // Store
            inst_info->rs1   = rs1;
            inst_info->rs2   = rs2;
            inst_info->imm   = imm_s; // imm12
            switch (func3)
            {
                case 0x0: inst_info->exec = (void*)exec_sb; break;
                case 0x1: inst_info->exec = (void*)exec_sh; break;
                case 0x2: inst_info->exec = (void*)exec_sw; break;
            }
            break;
        }
        case 0x13 : {       // ADDI..
            inst_info->rs1   = rs1;
            inst_info->rd    = rd;
            inst_info->imm   = imm_i; // imm12
            inst_info->shamt = (func3 == 0x1 || func3 == 0x5) ? rs2 : 0;
            switch (func3)
            {
                case 0x0: inst_info->exec = (void*)exec_addi;  break;
                case 0x1: inst_info->exec = (void*)exec_slli;  break;
                case 0x2: inst_info->exec = (void*)exec_slti;  break;
                case 0x3: inst_info->exec = (void*)exec_sltiu; break;
                case 0x4: inst_info->exec = (void*)exec_xori;  break;
                case 0x5: inst_info->exec = BIT(func7, 5) ? (void*)exec_srai : (void*)exec_srli; break;
                case 0x6: inst_info->exec = (void*)exec_ori;   break;
                case 0x7: inst_info->exec = (void*)exec_andi;  break;
            }
            break;
        }
        case 0x33 : {       // ADD..
            inst_info->rs1   = rs1;
            inst_info->rs2   = rs2;
            inst_info->rd    = rd;
#ifdef RV32M
            if ( BIT(func7, 0) )
            {
                switch (func3)
                {
                    case 0x0: inst_info->exec = (void*)exec_mul;    break;
                    case 0x1: inst_info->exec = (void*)exec_mulh;   break;
                    case 0x2: inst_info->exec = (void*)exec_mulhsu; break;
                    case 0x3: inst_info->exec = (void*)exec_mulhu;  break;
                    case 0x4: inst_info->exec = (void*)exec_div;    break;
                    case 0x5: inst_info->exec = (void*)exec_divu;   break;
                    case 0x6: inst_info->exec = (void*)exec_rem;    break;
                    case 0x7: inst_info->exec = (void*)exec_remu;   break;
                }
            }
            else
#endif // #ifdef RV32M
            {
                switch (func3)
                {
                    case 0x0: inst_info->exec = BIT(func7, 5) ? (void*)exec_sub : (void*)exec_add; break;
                    case 0x1: inst_info->exec = (void*)exec_sll;  break;
                    case 0x2: inst_info->exec = (void*)exec_slt;  break;
                    case 0x3: inst_info->exec = (void*)exec_sltu; break;
                    case 0x4: inst_info->exec = (void*)exec_xor;  break;
                    case 0x5: inst_info->exec = BIT(func7, 5) ? (void*)exec_sra : (void*)exec_srl; break;
                    case 0x6: inst_info->exec = (void*)exec_or;   break;
                    case 0x7: inst_info->exec = (void*)exec_and;  break;
                }
            }
            break;
        }
        case 0x0f : {       // FENCE, not support yet!
            inst_info->exec = (void*)exec_fence;
            break;
        }
        case 0x73 : {       // MRET/CSR..
            inst_info->rs1   = BIT(func3, 2) ? 0 : rs1;
            inst_info->rd    = rd;
            inst_info->imm   = BIT(func3, 2) ? rs1 : 0; // imm5
            inst_info->csr   = imm_i;
            switch (func3)
            {
                case 0x0: {
                    if ( 0x302 == inst_info->csr ) inst_info->exec = (void*)exec_mret; break;
                    break;
                }
                case 0x1: inst_info->exec = (void*)exec_csrrw;  break;
                case 0x2: inst_info->exec = (void*)exec_csrrs;  break;
                case 0x3: inst_info->exec = (void*)exec_csrrc;  break;
                case 0x5: inst_info->exec = (void*)exec_csrrwi; break;
                case 0x6: inst_info->exec = (void*)exec_csrrsi; break;
                case 0x7: inst_info->exec = (void*)exec_csrrci; break;
            }
            break;
        }
    }

    return 0;
}

/**
  * @brief  Instruction Fetching
  * @note   
  * @param  *inst_info, instruction information
  *         *mem, memory to store instruction
  * @retval int
  */
int ifu(InstInfo_t *inst_info, Memory_t *mem)
{
    // Note:
    //  memory is index by word
    //  pc is index by byte

    inst_info->inst = mem->buf[inst_info->current_pc/4];

#ifdef DEBUG_PRINT_PC
    DEBUG_D("pc: %08x inst: %08x\n", _pc, insti_info->inst);
#endif

    return 0;
}

/**
  * @brief  Instruction Decoding
  * @note   Regfile and Csr Regfile is Read in This Module
  * @param  *inst_info, instruction information
  * @retval int
  */
int idu(InstInfo_t *inst_info)
{
    _inst_decode(inst_info->inst, inst_info);

    _regfile_read(inst_info);
    _csrfile_read(inst_info);

    return 0;
}

/**
  * @brief  Instruction Excuting
  * @note   
  * @param  *inst_info, instruction information
  * @retval int
  */
int exu(InstInfo_t *inst_info)
{
    if ( inst_info->exec )
    {
        inst_info->rd_data = inst_info->exec((void*)inst_info);
    } else {
        inst_info->rd_data = 0;
    }

    return 0;
}

/**
  * @brief  Core Local Interrupt Controller
  * @note   
  * @param  *inst_info, instruction information
  * @retval int
  */
int clint(InstInfo_t *inst_info)
{
    if ( 0xffffffff == csrfile.buf[CSR_CYCLE] )
    {
        csrfile.buf[CSR_CYCLEH] ++;
        csrfile.buf[CSR_CYCLE] = 0;
    }
    csrfile.buf[CSR_CYCLE] ++;

    if ( BIT(_s_irq, 0) && BIT(csrfile.buf[CSR_MSTATUS], 3) )
    {
        uint32_t _mstatus = csrfile.buf[CSR_MSTATUS];

        csrfile.buf[CSR_MEPC]     = inst_info->next_pc;

        csrfile.buf[CSR_MSTATUS] &= ~(0x00000088);
        csrfile.buf[CSR_MSTATUS] |= SHIFT_L(BIT(_mstatus, 3), 7);

        csrfile.buf[CSR_MCAUSE]   = SHIFT_L(1, 31);
        csrfile.buf[CSR_MCAUSE]  |= BIT(_s_irq, 8) ? 8 : 0; // only timer1+uart1 irq!

        inst_info->next_pc = csrfile.buf[CSR_MTVEC];

        _s_irq = 0;
    }

    if ( inst_info->mret )
    {
        uint32_t _mstatus = csrfile.buf[CSR_MSTATUS];

        csrfile.buf[CSR_MSTATUS] |= SHIFT_L(BIT(_mstatus, 7), 3);
        csrfile.buf[CSR_MSTATUS] |= SHIFT_L(1, 7);

        inst_info->next_pc = csrfile.buf[CSR_MEPC];

        inst_info->mret = 0;
    }

    return 0;
}

/**
  * @brief  Memory Access
  * @note   Only Read and Write Memory
  * @param  *inst_info, instruction information
  * @retval int
  */
int mau(InstInfo_t *inst_info)
{
    if ( inst_info->mem_we && inst_info->mem_rd )
    {
        _dumpreg("invalid memory operate!", inst_info);
        return -1;
    }

    MemoryMap_t *pm = _find_by_addr(inst_info->mem_addr);

    if ( !pm )
    {
        _dumpreg("invalid memory device!", inst_info);
        return -1;
    }

    uint32_t addr;
    uint32_t offset;
    uint32_t dummy;

    addr = DATA_MASK(inst_info->mem_addr, 28, 0);

    if ( addr >= MEMORY_SIZE )
    {
        _dumpreg("invalid memory address!", inst_info);
        return -1;
    }

    if ( inst_info->mem_we )
    {
        dummy = pm->memory->buf[addr/4];
        switch (inst_info->mem_size)
        {
            case eMemorySize_Word     : {
                break;
            }
            case eMemorySize_HalfWord : {
                uint32_t mask = 0xffff;
                offset = BIT(addr, 1);
                mask = SHIFT_L(mask, offset*16);
                dummy &= ~mask;
                inst_info->mem_data = ~mask & dummy
                                    |  mask & SHIFT_L(inst_info->mem_data, offset*16);
                break;
            }
            case eMemorySize_Byte     : {
                uint32_t mask = 0xff;
                offset = addr % 4;
                mask = SHIFT_L(mask, offset*8);
                inst_info->mem_data = ~mask & dummy
                                    |  mask & SHIFT_L(inst_info->mem_data, offset*8);
                break;
            }
        }
        pm->memory->buf[addr/4] = inst_info->mem_data;

        goto _mau_exit;
    }

    if ( inst_info->mem_rd )
    {
        dummy = pm->memory->buf[addr/4];
        switch (inst_info->mem_size)
        {
            case eMemorySize_Word     : {
                inst_info->rd_data = dummy;
                break;
            }
            case eMemorySize_HalfWord : {
                offset = BIT(addr, 1);
                dummy = SHIFT_R(dummy, offset*16) & 0xffff;
                inst_info->rd_data = inst_info->mem_ext ? SIGN_EXTEND(dummy, 16)
                                                        : ZERO_EXTEND(dummy, 16);
                break;
            }
            case eMemorySize_Byte     : {
                offset = addr % 4;
                dummy = SHIFT_R(dummy, offset*8) & 0xff;
                inst_info->rd_data = inst_info->mem_ext ? SIGN_EXTEND(dummy, 8)
                                                        : ZERO_EXTEND(dummy, 8);
                break;
            }
        }

        goto _mau_exit;
    }

_mau_exit:
    return 0;
}

/**
  * @brief  Retire
  * @note   
  * @param  *inst_info, instruction information
  * @retval int
  */
int rtu(InstInfo_t *inst_info)
{
    _regfile_write(inst_info);
    _csrfile_write(inst_info);

    return 0;
}

/**
  * @brief  Define Peripheral Function
  * @note
  * @param  name, peripheral name
  *         *inst_info, instruction information
  * @retval MemoryMap_t*
  */

#ifdef ENABLE_TIMER
#include "./perips/timer/timer.c"
#endif
#ifdef ENABLE_UART
#include "./perips/uart/uart.c"
#endif

/**
  * @brief  Keyboard 
  * @note   Exec per 10ms
  * @param  *tid, thread id
  * @retval void*
  */

#ifdef __linux__

static struct termios _s_term;

static int _kbinit(void)
{
    struct termios term;

    tcgetattr(0, &_s_term);
    tcgetattr(0, &term);

    term.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(0, TCSANOW, &term);

    setbuf(stdin, NULL);

    return 0;
}

static int _kbrecover(void)
{
    tcsetattr(0, TCSANOW, &_s_term);
    return 0;
}

static int _kbhit(void)
{
    int rslt;

    ioctl(0, FIONREAD, &rslt);

    return rslt;
}

static uint8_t _getch(void)
{
    return (uint8_t)getchar();
}

#endif // #ifdef __linux__

static void* _keyboard(void *tid)
{
    MemoryMap_t *pm = _find_by_name("uart1");

    if ( !pm ) {
        return 0;
    }

    int loop = -1;

    static uint8_t _key_list[3];
    static uint8_t _key_idx = GET_ARRAY_NUM(_key_list);

    while ( loop )
    {
        extern void usleep(int);
        usleep(10*1000);

        _KB_LOCK();

        if ( _kbhit() )
        {
            uint8_t _key = (uint8_t)_getch();

            if ( _key == 0x04 ) // Ctrl + d/B
            {
                if ( _s_cli.mode )
                {
                    _s_cli.mode = 0;
                    _s_cli.debug = 0;

                    CLEAR_STRUCT(_s_cli.cmd, 0);

                    DEBUG_D("\n[INFO] exit  debug mode.\n");
                } else {
                    _s_cli.mode = 1;
                    _s_cli.debug = 1;

                    DEBUG_D(  "[INFO] enter debug mode.\n>>> ");
                    fflush(stdout);
                }

                goto _keyboard_continue;
            }

            if ( _s_cli.mode )
            {
                shell_getline(&_s_cli, _key);
                fflush(stdout);
            } else {
                switch ( _key )
                {
                    case 0x02 : { // Ctrl + b/B
                        if ( !_s_cli.mode ) {
                            loop = 0;
                        }
                        break;
                    }
                    case 0x08 : { // Backspace
                        pm->memory->buf[UART_REG_RXD/4] = ZERO_EXTEND(_key, 8);
                        pm->memory->buf[UART_REG_SR/4] |= SHIFT_L(1, 1);
                        _s_irq |= INT_IRQ_UART1;
                        break;
                    }
#ifdef __linux__
                    case 0x1b: { // Up Down Left Right
                        _key_idx = 0;
                        _key_list[0] = 0x1b;
                        _key_list[1] = _getch();
                        _key_list[2] = _getch();
                        break;
                    }
#else
                    case 0xe0: { // Up Down Left Right
                        _key_idx = 0;
                        _key_list[0] = 0x1b;
                        _key_list[1] = 0x5b;
                        switch ( _getch() )
                        {
                            case 0x48 : _key_list[2] = 0x41; break; // Up
                            case 0x50 : _key_list[2] = 0x42; break; // Down
                            case 0x4d : _key_list[2] = 0x43; break; // Right
                            case 0x4b : _key_list[2] = 0x44; break; // Left
                        }
                        break;
                    }
#endif // #ifdef __linux__
                    default  : {
                        pm->memory->buf[UART_REG_RXD/4] = ZERO_EXTEND(_key, 8);
                        pm->memory->buf[UART_REG_SR/4] |= SHIFT_L(1, 1);
                        _s_irq |= INT_IRQ_UART1;
                        break;
                    }
                }
            }
        }
        else if ( !_s_cli.mode )
        {
            if ( _key_idx < GET_ARRAY_NUM(_key_list) && !BIT(pm->memory->buf[UART_REG_SR/4], 1) )
            {
                pm->memory->buf[UART_REG_RXD/4] = ZERO_EXTEND(_key_list[_key_idx], 8);
                pm->memory->buf[UART_REG_SR/4] |= SHIFT_L(1, 1);
                _s_irq |= INT_IRQ_UART1;
                _key_idx ++;
            }
        }

_keyboard_continue:
        _KB_UNLOCK();
    } // while

    _s_quit = 1;

    pthread_exit(0);
    return 0;
}

/**
  * @brief  Check Value
  * @note   Stop Simulation while _check() Return '1'
  * @param  void
  * @retval int
  */
static int _check(void)
{
    int rslt = 0;

    if ( _s_quit ) {
        // Ctrl + B: exit xemu sim
        return 1;
    }

#ifdef DEBUG_ISA
    // x26 = 1, x27 = 1: PASS
    // x26 = 1, x27 = 0: FAIL
    if ( _s_trig )
    {
        DEBUG_D("\nISA TEST RESULT: %s\n", iregfile.buf[27] ? "PASS." : "FAIL!");
        rslt = 1;
    }
    _s_trig = iregfile.buf[26];
#endif // #ifdef DEBUG_ISA

#ifdef ENABLE_UART
    MemoryMap_t *pm = _find_by_name("uart1");

    if ( !pm ) {
        return -1;
    }

    if ( 0x1b == _s_trig )
    {
        // sequence 0x1b 0x04: stop simulation
        if ( 0x4 == DATA_MASK(pm->memory->buf[UART_REG_TXD/4], 8, 0) ) {
            rslt = 1;
        }
    }
    _s_trig = DATA_MASK(pm->memory->buf[UART_REG_TXD/4], 8, 0);
#endif // #ifdef ENABLE_UART

    return rslt;
}

/**
  * @brief  Debug
  * @note   
  * @param  *inst_info, instruction information
  * @retval int
  */
static int _debug(InstInfo_t *inst_info)
{
    if ( !_s_cli.debug ) {
        return 0;
    }

    while ( _s_cli.debug && !_s_cli.cmd.submit )
    {
        extern void usleep(int);
        usleep(10*1000);
    }

    _KB_LOCK();

    char *ptr = _s_cli.cmd.command;
    char *endptr;

    int opt = (ptr[0] | 0x20); // NOTE: only support single-char options
    int num1, num2;

    switch (opt)
    {
        case 'm': // NOTE: dump memory
        case 'x': // NOTE: dump memory
            num1 = strtol(&ptr[1], &endptr, 16);
            if ( *endptr == ' ' )
            {
                num2 = strtol(endptr, &endptr, 10);
                if ( *endptr == '\0' )
                {
                    DEBUG_D("\nmemory address 0x%08x, length %d.", num1, num2);
                    _dumpmem("", num1, num2);
                }
            }
            break;
        case 'r': // NOTE: dump register
            _dumpreg(NULL, inst_info);
            _s_quit = 0;
            break;
        case 'n': // NOTE: step trace
        case 's': // NOTE: step next
            DEBUG_D("\npc [%08x] memory [%08x]\n", inst_info->current_pc, inst_info->mem_addr);
            break;
        case 0x20:
            break;
        default:
            DEBUG_D("\nunknown command '%c'.\n", opt);
            break;
    }

    if ( _s_cli.debug )
    {
        DEBUG_D("\n>>> ");
        fflush(stdout);
    }

    _s_cli.cmd.submit = 0;

    _KB_UNLOCK();

    return 0;
}

/**
  * @brief  Main Task
  * @note
  * @param  argc, number of parameter
  *         *argv[], parameter
  * @retval int
  */

int main(int argc, char *argv[])
{
    cli_parse(&_s_cli, argc, argv);

    if ( !_s_cli.run ) {
        return 0;
    }

    _s_cli.filename = _s_cli.arg_filename;
    _s_cli.debug = _s_cli.arg_debug;
    _s_cli.trace = _s_cli.arg_trace;

    memory_init();

    if ( load_image(_s_cli.filename, &rom) ) {
        return 0;
    }

    pthread_t tkb;
    pthread_create(&tkb, NULL, _keyboard, NULL);

    pthread_mutex_init(&_s_kb_mutex, 0);

#ifdef __linux__
    _kbinit();
#endif

    while (0x1754)
    {
        static uint32_t _pc = 0;

        InstInfo_t inst_info;
        CLEAR_STRUCT(inst_info, 0);

        inst_info.current_pc = _pc;
        inst_info.next_pc = _pc + 4;

        { // core task
            ifu(&inst_info, &rom);

            idu(&inst_info);

            exu(&inst_info);

            _KB_LOCK();
            {
                clint(&inst_info);
                mau(&inst_info);
            }
            _KB_UNLOCK();

            rtu(&inst_info);
        }

        if ( _s_cli.trace ) {
            _dumpreg(NULL, &inst_info);
            _s_quit = 0;
        }

        { // periphs task
            _KB_LOCK();
#ifdef ENABLE_TIMER
            _timer("timer", &inst_info);
#endif
#ifdef ENABLE_UART
            _uart("uart1", &inst_info);
#endif
            _KB_UNLOCK();
        }

        if ( _check() ) {
            break;
        }

        _pc = inst_info.next_pc;

        _debug(&inst_info); // NOTE: pc has updated
    } // while (1)

#ifdef __linux__
    _kbrecover();
#endif

    DEBUG_D("\nxemu quit..\n");

    return 0;
}
